/*
* Copyright (c) 2025 Shenzhen Kaihong Digital Industry Development Co., Ltd.
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
const { XButton } = require("../../engine/control/XButton");
const { X3DFast, X2DBeta } = require("../../engine/graphics/X2DInterface");
const { XFbo } = require("../../engine/graphics/XFbo");
const { XMat4 } = require("../../engine/graphics/XMat4");
const { XVec4 } = require("../../engine/graphics/XVec4");
const { Scr } = require("../../engine/XDefine");
const { XTools } = require("../../engine/XTools");
const { AttributeArea } = require("../AttributeArea");
const { LogManager } = require("../ros1/LogManager");
const { SaveRobotConfig } = require("./SaveRobotConfig");
const { URDF } = require("./URDF");

function findFootOfPerpendicular(x1, y1, x2, y2) {
    // 计算点积 v1 · v2
    const dotProduct = x1 * x2 + y1 * y2;
    // 计算 v2 的模的平方 ||v2||^2
    const normSquared = x2 * x2 + y2 * y2;

    // 避免除以零（如果 x2 和 y2 都为 0，则 v2 是零向量，无意义）
    if (normSquared === 0) {
        return [0, 0];
    }

    // 计算垂足坐标
    const footX = (dotProduct / normSquared) * x2;
    const footY = (dotProduct / normSquared) * y2;

    return [footX, footY];
}

class EditRobot {
    static getInstance() {
        if (!EditRobot.instance) {
            EditRobot.instance = new EditRobot();
        }
        return EditRobot.instance;
    }
    constructor() {
        this.on = false;
        this.tmat = new XMat4();
        this.coordinate = {
            mat: new XMat4(),
            pos: { x: 0, y: 0, z: 0 },
            rotate: { x: 15, y: 0, z: -15, },
            scale: 100
        };
        this.coordinateImpl();
        this.free = {
            count: 0,
            btns: [],
        }
        this.run = false;
        this.partPoint = -1;
        this.pickPoint = -1;
        this.isOrtho = false;
        this.fbo = new XFbo({
            w: Scr.logicw,
            h: Scr.logich,
        });
        this.btn_adjust = [
            new XButton({
                onDraw: () => {
                    X2DBeta.Circle(this.btn_adjust[0].centerX(), this.btn_adjust[0].centerY(), 15, 15, 0, 360, 0xff000000 | X3DFast.BLENDER_RED);
                    X2DBeta.drawText("X", 14, this.btn_adjust[0].centerX(), this.btn_adjust[0].centerY(), 1, 1, 0, -2, -2, 0xff000000);
                },
                onDrag: (x, y) => {
                    let part = this.robot.parts[this.partPoint];
                    part.xyz.x = this.fix(part.xyz.x + x * this.btn_adjust[0].dx);
                    this.reset("edit_part");
                    // let foot = findFootOfPerpendicular(x, y, this.btn_adjust[0].dx, this.btn_adjust[0].dy);
                    // let length = Math.sqrt(foot[0] * foot[0] + foot[1] * foot[1]);
                    // part.xyz.x += (this.btn_adjust[0].dx * x > 0 ? 1 : -1) * length * this.btn_adjust[0].dz;
                }
            }),
            new XButton({
                onDraw: () => {
                    X2DBeta.Circle(this.btn_adjust[1].centerX(), this.btn_adjust[1].centerY(), 15, 15, 0, 360, 0xff000000 | X3DFast.BLENDER_GREEN);
                    X2DBeta.drawText("Y", 14, this.btn_adjust[1].centerX(), this.btn_adjust[1].centerY(), 1, 1, 0, -2, -2, 0xff000000);
                },
                onDrag: (x, y) => {
                    let part = this.robot.parts[this.partPoint];
                    part.xyz.y = this.fix(part.xyz.y + x * this.btn_adjust[1].dx);
                    this.reset("edit_part");
                }
            }),
            new XButton({
                onDraw: () => {
                    X2DBeta.Circle(this.btn_adjust[2].centerX(), this.btn_adjust[2].centerY(), 15, 15, 0, 360, 0xff000000 | X3DFast.BLENDER_BLUE);
                    X2DBeta.drawText("Z", 14, this.btn_adjust[2].centerX(), this.btn_adjust[2].centerY(), 1, 1, 0, -2, -2, 0xff000000);
                },
                onDrag: (x, y) => {
                    let part = this.robot.parts[this.partPoint];
                    part.xyz.z = this.fix(part.xyz.z + y * this.btn_adjust[2].dy);
                    this.reset("edit_part");
                }
            }),
        ];
    }
    fix(f) {
        return Math.round(f * 1000) / 1000;
    }
    coordinateImpl() {
        this.coordinate.mat.Unit();
        this.coordinate.mat.Scale(this.coordinate.scale, this.coordinate.scale, this.coordinate.scale);
        this.coordinate.mat.Rotate(0, 0, this.coordinate.rotate.z);
        this.coordinate.mat.Rotate(this.coordinate.rotate.x, 0, 0);
        this.coordinate.mat.Move(0, 0, 0);
    }
    init(rb) {
        this.robot = rb;
    }
    open() {
        this.on = true;
        this.reset("edit_robot");
    }
    reset(stat) {
        AttributeArea.gi().clear();
        switch (stat) {
            case "edit_robot":
                AttributeArea.gi().addTitle("编辑仿真机器人");
                // AttributeArea.gi().addGap(1);
                AttributeArea.gi().addInput({
                    label: "名称：",
                    default: this.robot.name,
                    callback: (value) => {
                        this.robot.name = value;
                        SaveRobotConfig.getInstance().saveConfig();
                    },
                });
                AttributeArea.gi().addGap(1);
                let pns = [];
                for (let p of this.robot.parts) {
                    pns.push(p.name + " - (" + (p.isRoot ? "主体)" : (p.parent == "" ? "未指定)" : "父部件：" + p.parent + ")")));
                }
                AttributeArea.gi().addList({
                    label: `部件列表(${pns.length})：`,
                    list: pns,
                    default: this.partPoint >= 0 ? pns[this.partPoint] : "",
                    callback: (value) => {
                        let pn = value.split(" - ")[0];
                        for (let i = 0; i < this.robot.parts.length; i++) {
                            if (this.robot.parts[i].name == pn) {
                                this.partPoint = i;
                                this.reset("edit_robot");
                                break;
                            }
                        }
                    },
                });
                if (this.partPoint >= 0) {
                    AttributeArea.gi().addGap(1);
                    AttributeArea.gi().addMultElements([
                        {
                            type: "button",
                            label: `编辑部件 ${this.robot.parts[this.partPoint].name} 的属性`,
                            callback: () => {
                                this.reset("edit_part");
                            }
                        }, {
                            type: "button",
                            label: `删除部件 ${this.robot.parts[this.partPoint].name}`,
                            callback: () => {
                                this.robot.parts.splice(this.partPoint, 1);
                                this.partPoint = -1;
                                this.reset("edit_robot");
                                SaveRobotConfig.getInstance().saveConfig();
                            }
                        }
                    ]);
                }
                AttributeArea.gi().addGap(1);
                AttributeArea.gi().addButton({
                    label: "选择部件添加到机器人",
                    callback: () => {
                        const { SelectRobotPart } = require("./SelectRobotPart");
                        SelectRobotPart.getInstance().open();
                    },
                });
                AttributeArea.gi().addGap(1);
                AttributeArea.gi().addButton({
                    label: "选择gazebo插件添加到机器人",
                    callback: () => {
                        LogManager.E("工能开发中！");
                    },
                });
                AttributeArea.gi().addGap(1);
                AttributeArea.gi().addButton({
                    label: "导出urdf文件",
                    callback: () => {
                        URDF.exportRobot(this.robot);
                    },
                });
                AttributeArea.gi().addCheckbox({
                    label: "运动总开关",
                    default: this.run,
                    callback: (value) => {
                        this.run = value;
                    },
                });
                break;
            case "edit_part":
                AttributeArea.gi().addTitle("机器人部件属性");
                let part = this.robot.parts[this.partPoint];
                AttributeArea.gi().addLabelInput({
                    label: "名称：",
                    default: part.name,
                    callback: (value) => {
                        if (value.length < 3 || value.length > 32) {
                            LogManager.E("部件名称长度必须在3-32之间！");
                            return;
                        }
                        if (!XTools.isValidVariableName(value)) {
                            LogManager.E("部件名称只能包含字母、数字、下划线，首字母不能为数字！");
                            return;
                        }
                        for (let p of this.robot.parts) {
                            if (p.name == value) {
                                LogManager.E("部件名称不能重复！");
                                return;
                            }
                        }
                        for (let p of this.robot.parts) {
                            if (p.parent == part.name) {
                                p.parent = value;
                            }
                        }
                        part.name = value;
                        SaveRobotConfig.getInstance().saveConfig();
                    },
                });
                AttributeArea.gi().addGap(1);

                let parents = ["", "主体(无父部件，可作为机器人的身体)"];
                for (let p of this.robot.parts) {
                    if (p != part && (p.isRoot || p.parent != "")) {//是主体，或者有父部件，可选择其成为我的父部件
                        parents.push(p.name);
                    }
                }
                AttributeArea.gi().addLabelDropdown({
                    label: "指定父部件：",
                    list: parents,
                    default: part.isRoot ? parents[1] : part.parent,
                    callback: (value) => {
                        if (value == parents[1]) {
                            part.isRoot = true;
                            part.parent = "";
                        } else {
                            part.isRoot = false;
                            part.parent = value;
                        }
                        this.reset("edit_part");
                        SaveRobotConfig.getInstance().saveConfig();
                    },
                });
                if (part.isRoot) {
                } else {
                    if (part.parent != "") {
                        const relatives = [
                            "revolute",
                            "continuous",
                            "prismatic",
                            "fixed",
                            "floating",
                            "planar",
                        ];
                        const relatives2 = [
                            "revolute - 绕轴旋转，有上下限",
                            "continuous - 绕轴旋转，无限旋转",
                            "prismatic - 沿轴滑动，有上下限",
                            "fixed - 固定",
                            "floating - 悬浮",
                            "planar - 垂直于轴的平面中运动",
                        ];
                        AttributeArea.gi().addGap(1);
                        AttributeArea.gi().addLabelDropdown({
                            label: "和父节点关系：",
                            list: relatives2,
                            default: relatives2[relatives.indexOf(part.jointType)],
                            callback: (value) => {
                                part.jointType = relatives[relatives2.indexOf(value)];
                                this.reset("edit_part");
                                SaveRobotConfig.getInstance().saveConfig();
                            },
                        });
                    }
                }
                AttributeArea.gi().addGap(1);
                if (part.isRoot) AttributeArea.gi().addLabel({ label: "位置:" });
                else AttributeArea.gi().addLabel({ label: "位置(相对父部件):" });
                AttributeArea.gi().addLabelInputs([
                    {
                        label: "x:",
                        default: part.xyz.x,
                        callback: (value) => {
                            part.xyz.x = parseFloat(value);
                            SaveRobotConfig.getInstance().saveConfig();
                        },
                    }, {
                        label: "y:",
                        default: part.xyz.y,
                        callback: (value) => {
                            part.xyz.y = parseFloat(value);
                            SaveRobotConfig.getInstance().saveConfig();
                        },
                    }, {
                        label: "z:",
                        default: part.xyz.z,
                        callback: (value) => {
                            part.xyz.z = parseFloat(value);
                            SaveRobotConfig.getInstance().saveConfig();
                        },
                    }
                ]);
                if (part.isRoot) AttributeArea.gi().addLabel({ label: "旋转:" });
                else AttributeArea.gi().addLabel({ label: "旋转(相对父部件):" });
                AttributeArea.gi().addLabelInputs([
                    {
                        label: "x:",
                        default: part.rpy.x,
                        callback: (value) => {
                            part.rpy.x = parseFloat(value);
                            SaveRobotConfig.getInstance().saveConfig();
                        },
                    }, {
                        label: "y:",
                        default: part.rpy.y,
                        callback: (value) => {
                            part.rpy.y = parseFloat(value);
                            SaveRobotConfig.getInstance().saveConfig();
                        },
                    }, {
                        label: "z:",
                        default: part.rpy.z,
                        callback: (value) => {
                            part.rpy.z = parseFloat(value);
                            SaveRobotConfig.getInstance().saveConfig();
                        },
                    }
                ]);
                if (["revolute", "continuous", "prismatic", "planar"].includes(part.jointType)) {
                    //axis
                    if (part.jointType == "revolute" || part.jointType == "continuous") {
                        AttributeArea.gi().addLabel({ label: "旋转轴向量:" });
                    } else if (part.jointType == "prismatic") {
                        AttributeArea.gi().addLabel({ label: "滑动轴向量:" });
                    } else if (part.jointType == "planar") {
                        AttributeArea.gi().addLabel({ label: "垂直于滑动平面的轴向量:" });
                    }
                    AttributeArea.gi().addLabelInputs([
                        {
                            label: "x:",
                            default: part.axis.x,
                            callback: (value) => {
                                part.axis.x = parseFloat(value);
                                SaveRobotConfig.getInstance().saveConfig();
                            },
                        }, {
                            label: "y:",
                            default: part.axis.y,
                            callback: (value) => {
                                part.axis.y = parseFloat(value);
                                SaveRobotConfig.getInstance().saveConfig();
                            },
                        }, {
                            label: "z:",
                            default: part.axis.z,
                            callback: (value) => {
                                part.axis.z = parseFloat(value);
                                SaveRobotConfig.getInstance().saveConfig();
                            },
                        }
                    ]);
                }
                if (["revolute", "prismatic"].includes(part.jointType)) {//limits
                    AttributeArea.gi().addLabel({ label: "关节限制:" });
                    if (part.jointType == "revolute") {//旋转关节
                        AttributeArea.gi().addLabelInputs([
                            {
                                label: "角度下限(度):",
                                default: part.limit.lower,
                                callback: (value) => {
                                    part.limit.lower = parseFloat(value);
                                    SaveRobotConfig.getInstance().saveConfig();
                                },
                            }, {
                                label: "角度上限(度):",
                                default: part.limit.upper,
                                callback: (value) => {
                                    part.limit.upper = parseFloat(value);
                                    SaveRobotConfig.getInstance().saveConfig();
                                },
                            }
                        ]);
                        AttributeArea.gi().addLabelInputs([
                            {
                                label: "角速度(度/秒):",
                                default: part.limit.velocity,
                                callback: (value) => {
                                    part.limit.velocity = parseFloat(value);
                                    SaveRobotConfig.getInstance().saveConfig();
                                },
                            }, {
                                label: "输出力矩(牛米):",
                                default: part.limit.effort,
                                callback: (value) => {
                                    part.limit.effort = parseFloat(value);
                                    SaveRobotConfig.getInstance().saveConfig();
                                },
                            }
                        ]);
                    } else if (part.jointType == "prismatic") {
                        AttributeArea.gi().addLabelInputs([
                            {
                                label: "滑动距离下限(米):",
                                default: part.limit.lower,
                                callback: (value) => {
                                    part.limit.lower = parseFloat(value);
                                    SaveRobotConfig.getInstance().saveConfig();
                                },
                            }, {
                                label: "滑动距离上限(米):",
                                default: part.limit.upper,
                                callback: (value) => {
                                    part.limit.upper = parseFloat(value);
                                    SaveRobotConfig.getInstance().saveConfig();
                                },
                            }
                        ]);
                        AttributeArea.gi().addLabelInputs([
                            {
                                label: "滑动速度(米/秒):",
                                default: part.limit.velocity,
                                callback: (value) => {
                                    part.limit.velocity = parseFloat(value);
                                    SaveRobotConfig.getInstance().saveConfig();
                                },
                            }, {
                                label: "输出力矩(牛米):",
                                default: part.limit.effort,
                                callback: (value) => {
                                    part.limit.effort = parseFloat(value);
                                    SaveRobotConfig.getInstance().saveConfig();
                                },
                            }
                        ]);
                    }
                }
                if (part.jointType == "continuous" || part.jointType == "revolute") {
                    AttributeArea.gi().addCheckbox({
                        label: "模拟运动",
                        default: part.runtime.run,
                        callback: (value) => {
                            part.runtime.run = value;
                            SaveRobotConfig.getInstance().saveConfig();
                        },
                    });
                }
                // "revolute - 绕轴旋转，有上下限",
                // "continuous - 绕轴旋转，无限旋转",
                // "prismatic - 沿轴滑动，有上下限",
                // "fixed - 固定",
                // "floating - 悬浮",
                // "planar - 垂直于轴的平面中运动",

                //<axis> 默认xyz="1 0 0"，和 revolute, continuous, prismatic, planar 相关
                //<limits> 关节的上下限 lower=0, upper=0, effort必需, velocity必需，和 revolute, prismatic 相关

                //下面的暂不添加：
                //<calibration> 校准关节的绝对位置 rising, falling
                //<dynamics> 动力学参数 damping=0物理阻尼，friction=0摩擦力
                //<mimic> 模拟其他关节
                //<safety_controller>
                break;
        }
        AttributeArea.gi().flush();
    }
    addPart(name, token) {//部件添加到机器人
        let pid = 1;
        let pname = name + "_" + pid;
        while (true) {
            let duplicated = false;
            for (let p of this.robot.parts) {
                if (p.name === pname) {
                    duplicated = true;
                    break;
                }
            }
            if (duplicated) {
                pid++;
                pname = name + "_" + pid;
            } else break;
        }
        this.robot.parts.push(
            {
                name: pname,
                token: token,
                isRoot: false,
                parent: "",
                jointType: "fixed",
                xyz: { x: 0, y: 0, z: 0 },
                rpy: { x: 0, y: 0, z: 0 },
                axis: { x: 1, y: 0, z: 0 },
                limit: { lower: 0, upper: 0, velocity: 0, effort: 100 },
                runtime: {
                    run: false,
                },//运动测试
            }
        );
        this.open();
        SaveRobotConfig.getInstance().saveConfig();
    }
    moveAdjustButton(part, mat) {
        let length = 200 / this.coordinate.scale;
        X3DFast.drawLines([0, 0, 0, length, 0, 0], mat, 0xff000000 | X3DFast.BLENDER_RED);
        X3DFast.drawLines([0, 0, 0, 0, length, 0], mat, 0xff000000 | X3DFast.BLENDER_GREEN);
        X3DFast.drawLines([0, 0, 0, 0, 0, length], mat, 0xff000000 | X3DFast.BLENDER_BLUE);
        const btnw = 30, btnh = 30;
        let vec = new XVec4();
        let pos, pos2, length2;

        vec.set(length, 0, 0);
        pos = vec.transform(mat);
        this.btn_adjust[0].move(pos[0] - btnw / 2, pos[1] - btnh / 2, btnw, btnh);
        vec.set(length * 2, 0, 0);
        pos2 = vec.transform(mat);
        this.btn_adjust[0].dx = (pos2[0] - pos[0]) == 0 ? 0 : length / (pos2[0] - pos[0]);
        this.btn_adjust[0].dy = (pos2[1] - pos[1]) == 0 ? 0 : length / (pos2[1] - pos[1]);
        length2 = Math.sqrt((pos2[0] - pos[0]) ** 2 + (pos2[1] - pos[1]) ** 2);
        this.btn_adjust[0].dz = length2 == 0 ? 0 : length / length2;


        vec.set(0, length, 0);
        pos = vec.transform(mat);
        this.btn_adjust[1].move(pos[0] - btnw / 2, pos[1] - btnh / 2, btnw, btnh);
        vec.set(0, length * 2, 0);
        pos2 = vec.transform(mat);
        this.btn_adjust[1].dx = (pos2[0] - pos[0]) == 0 ? 0 : length / (pos2[0] - pos[0]);
        this.btn_adjust[1].dy = (pos2[1] - pos[1]) == 0 ? 0 : length / (pos2[1] - pos[1]);
        length2 = Math.sqrt((pos2[0] - pos[0]) ** 2 + (pos2[1] - pos[1]) ** 2);
        this.btn_adjust[1].dz = length2 == 0 ? 0 : length / length2;

        vec.set(0, 0, length);
        pos = vec.transform(mat);
        this.btn_adjust[2].move(pos[0] - btnw / 2, pos[1] - btnh / 2, btnw, btnh);
        vec.set(0, 0, length * 2);
        pos2 = vec.transform(mat);
        this.btn_adjust[2].dx = (pos2[0] - pos[0]) == 0 ? 0 : length / (pos2[0] - pos[0]);
        this.btn_adjust[2].dy = (pos2[1] - pos[1]) == 0 ? 0 : length / (pos2[1] - pos[1]);
        length2 = Math.sqrt((pos2[0] - pos[0]) ** 2 + (pos2[1] - pos[1]) ** 2);
        this.btn_adjust[2].dz = length2 == 0 ? 0 : length / length2;
    }
    renderPart(part, mat) {
        const { SelectRobotPart } = require("./SelectRobotPart");
        X3DFast.swapMode3D(this.isOrtho);

        let tm = new XMat4();
        tm.Rotate(part.rpy.x, part.rpy.y, part.rpy.z);
        tm.Move(part.xyz.x, part.xyz.y, part.xyz.z);
        tm.Mult(mat);
        if (this.pickPoint > 0) {
            part.pp = this.pickPoint;
        } else if (this.partPoint >= 0 && this.robot.parts[this.partPoint] == part) {
            this.moveAdjustButton(part, tm);
            if (part.jointType == "continuous" || part.jointType == "revolute") {//绘制旋转轴
                let vec = new XVec4(part.axis.x, part.axis.y, part.axis.z, 0);
                vec.normalize();
                X3DFast.drawLines([-1 * vec.vec[0], -1 * vec.vec[1], -1 * vec.vec[2], 1 * vec.vec[0], 1 * vec.vec[1], 1 * vec.vec[2]], tm, 0xffffffff);
            }
        }
        if ((this.run || this.robot.parts[this.partPoint] == part) && part.runtime.run) {// 模拟运动 this.partPoint >= 0 && this.robot.parts[this.partPoint] == part && 
            if (part.jointType == "continuous") {
                let vec = new XVec4(part.axis.x, part.axis.y, part.axis.z, 0);
                let rm = vec.makeRotateMat(-XTools.TIME_MS / 1000);
                tm.MultRight(rm);
            } else if (part.jointType == "revolute") {
                let area = (part.limit.upper - part.limit.lower);
                let angle = (parseInt((XTools.TIME_MS / 1000) * part.limit.velocity * 100) % (area * 2 * 100)) / 100;
                if (angle > area) angle = area * 2 - angle;
                angle += part.limit.lower;
                let vec = new XVec4(part.axis.x, part.axis.y, part.axis.z, 0);
                let rm = vec.makeRotateMat(-angle * 3.1415926 / 180);
                tm.MultRight(rm);
            }
        }
        SelectRobotPart.getInstance().renderCustomPart3(part.token, tm, this.partPoint >= 0 && this.robot.parts[this.partPoint] == part, this.pickPoint <= 0 ? 0 : this.pickPoint++);
        for (let p of this.robot.parts) {
            if (p.parent == part.name) {
                this.renderPart(p, tm);
            }
        }
    }
    pick() {
        //使用fbo再渲染一遍，然后读取像素值判断是否在物体内
        this.pickPoint = 1;//启动拾取
        this.fbo.reInit(Scr.logicw, Scr.logich);
        this.fbo.Begin();
        X3DFast.swapMode3D(this.isOrtho);
        for (let i = 0; i < this.robot.parts.length; i++) {
            let part = this.robot.parts[i];
            if (part.isRoot) {//是主体，渲染
                this.renderPart(part, this.coordinate.mat);
            }
        }
        this.fbo.End();
        this.pickPoint = -1;

        let pixel = this.fbo.getPixel(XTools.MOUSE_POS.x, Scr.logich - XTools.MOUSE_POS.y - 1);
        if (pixel[2] == 0) {
            let pp = pixel[0] + pixel[1] * 256;
            for (let i = 0; i < this.robot.parts.length; i++) {
                if (this.robot.parts[i].pp == pp) {
                    return i;
                }
            }
        }
        // {
        //     X2DBeta.swapMode2D();
        //     let scale = 0.3;
        //     this.fbo.Draw(100, 100 + Scr.logich * scale, scale, -scale, 0, -1, -1, 0xffffffff);
        //     let dx = 100 + XTools.MOUSE_POS.x * scale;
        //     let dy = 100 + XTools.MOUSE_POS.y * scale;
        //     X2DBeta.drawLine(dx, dy, dx + 30, dy, 0xff00ff00);
        //     X2DBeta.drawLine(dx, dy, dx, dy + 30, 0xff000000);
        //     X2DBeta.drawText(pixel.toString(), 14, dx + 10, dy + 10, 1, 1, 0, -2, -2, 0xffff0000);
        // }
        return -1;
    }
    pick2(part, mat) {
        const { SelectRobotPart } = require("./SelectRobotPart");
        X3DFast.swapMode3D(this.isOrtho);

        let tm = new XMat4();
        if (this.partPoint >= 0 && this.robot.parts[this.partPoint] == part && this.robot.parts[this.partPoint].jointType == "continuous") {//沿轴旋转
            let vec = new XVec4(part.axis.x, part.axis.y, part.axis.z, 0);
            let rm = vec.makeRotateMat(XTools.TIME_MS / 1000);
            tm.Mult(rm);
        }
        tm.Rotate(part.rpy.x, part.rpy.y, part.rpy.z);
        tm.Move(part.xyz.x, part.xyz.y, part.xyz.z);
        tm.Mult(mat);

        let vec0 = new XVec4(0, -Scr.logicw / 2 / Math.tan(30 * 3.1415926 / 180), 0, 1);//在投影矩阵下的视点坐标
        vec0.mult(X3DFast.umat);
        let vec1 = new XVec4((XTools.MOUSE_POS.x) / Scr.logicw * 2 - 1, 1 - XTools.MOUSE_POS.y / Scr.logich * 2, 0, 0);
        // vec1.mult(X3DFast.umat.Inverse());//屏幕2d坐标转换到投影矩阵下的鼠标坐标
        if (SelectRobotPart.getInstance().pick(
            {
                token: part.token,
                mat: tm,
                v0: vec0,
                v1: vec1,
            }
        )) {
            this.partPoint = this.robot.parts.indexOf(part);
            return true;
        }

        for (let p of this.robot.parts) {
            if (p.parent == part.name) {
                if (this.pick(p, tm)) {
                    return true;
                }
            }
        }
        return false;
    }
    onDraw() {
        X3DFast.swapMode3D(this.isOrtho);
        X3DFast.coordinateLine(this.coordinate.mat, 5);

        this.free.count = 0;
        for (let i = 0; i < this.robot.parts.length; i++) {
            let part = this.robot.parts[i];
            if (part.isRoot) {//是主体，渲染
                this.renderPart(part, this.coordinate.mat);
            }
            else if (part.parent == "") {
                if (this.free.count >= this.free.btns.length) {
                    let btn = new XButton({
                        onClicked: () => {
                            this.partPoint = btn.pp;
                            this.reset("edit_part");
                        }
                    });
                    this.free.btns.push(btn);
                }
                let dx = 100;
                let dy = 100 + 100 * this.free.count;
                this.free.btns[this.free.count].move(dx - 50, dy - 50, 100, 100);
                this.free.btns[this.free.count].pp = i;
                {
                    const { SelectRobotPart } = require("./SelectRobotPart");
                    X3DFast.swapMode3D(true);
                    SelectRobotPart.getInstance().renderCustomPart(part.token, dx, dy);//未设置父节点，在左上角显示
                    X2DBeta.swapMode2D();
                    X2DBeta.drawText(part.name, 14, dx, dy + 35, 1, 1, 0, -2, -2, 0xffffffff);
                    if (this.partPoint == i) {
                        X2DBeta.drawRect(dx - 50, dy - 50, 100, 100, 0xff00ff00, 3);
                    } else {
                        X2DBeta.drawRect(dx - 50, dy - 50, 100, 100, 0xffffffff, 1);
                    }
                    X2DBeta.Flush();
                }
                this.free.count++;
            }
            // this.renderPart(part, this.coordinate.mat);
        }
        X2DBeta.swapMode2D();

        X3DFast.coordinateLetter(this.coordinate.mat, 5);
        if (this.partPoint >= 0) this.drawAdjustBtn();
    }
    drawAdjustBtn() {
        for (let btn of this.btn_adjust) {
            btn.onDraw();
        }
    }
    onTouch(msg, x, y) {
        for (let i = 0; i < this.free.count; i++) {
            if (this.free.btns[i].onTouch(msg, x, y)) return true;
        }
        if (msg == 10) {
            this.coordinate.scale = this.coordinate.scale * 1.1;
            this.coordinateImpl();
        }
        if (msg == 11) {
            this.coordinate.scale = this.coordinate.scale * 0.9;
            this.coordinateImpl();
        }
        if (this.partPoint >= 0) {
            for (let btn of this.btn_adjust) {
                if (btn.onTouch(msg, x, y)) {
                    return true;
                }
            }
        }
        if (this.drag) {
            if (msg == 2) {
                let dx = x - this.drag.x;
                let dy = y - this.drag.y;
                this.coordinate.rotate.z += dx / 10;
                this.coordinate.rotate.x += dy / 10;
                this.coordinateImpl();
                this.drag = {
                    x: x,
                    y: y,
                    draged: true,
                }
            } else if (msg == 3) {
                if (!this.drag.draged) {
                    let pp = this.pick();
                    if (pp != this.partPoint) {
                        this.partPoint = pp;
                        if (pp == -1) this.reset("edit_robot");
                        else this.reset("edit_part");
                    }
                }
                this.drag = false;
            }
        }

        if (msg == 1) {
            this.drag = {
                x: x,
                y: y,
                draged: false,
            }
        }
        return true;
    }
}

module.exports = {
    EditRobot
};